// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
// details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <svo/feature.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <vikit/math_utils.h>
#include <stdexcept>

namespace svo {

int Point::point_counter_ = 0;

Point::Point(const Vector3d& pos)
    : id_(point_counter_++),
      pos_(pos),
      normal_set_(false),
      n_obs_(0),
      v_pt_(NULL),
      last_published_ts_(0),
      last_projected_kf_id_(-1),
      type_(TYPE_UNKNOWN),
      n_failed_reproj_(0),
      n_succeeded_reproj_(0),
      last_structure_optim_(0) {}

Point::Point(const Vector3d& pos, Feature* ftr)
    : id_(point_counter_++),
      pos_(pos),
      normal_set_(false),
      n_obs_(1),
      v_pt_(NULL),
      last_published_ts_(0),
      last_projected_kf_id_(-1),
      type_(TYPE_UNKNOWN),
      n_failed_reproj_(0),
      n_succeeded_reproj_(0),
      last_structure_optim_(0) {
  obs_.push_front(ftr);
}

Point::~Point() {}

void Point::addFrameRef(Feature* ftr) {
  obs_.push_front(ftr);
  ++n_obs_;
}

Feature* Point::findFrameRef(Frame* frame) {
  for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it)
    if ((*it)->frame == frame)
      return *it;
  return NULL;  // no keyframe found
}

bool Point::deleteFrameRef(Frame* frame) {
  for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) {
    if ((*it)->frame == frame) {
      obs_.erase(it);
      return true;
    }
  }
  return false;
}

void Point::initNormal() {
  assert(!obs_.empty());
  const Feature* ftr = obs_.back();
  assert(ftr->frame != NULL);
  normal_ = ftr->frame->T_f_w_.rotationMatrix().transpose() * (-ftr->f);
  normal_information_ = DiagonalMatrix<double, 3, 3>(
      pow(20 / (pos_ - ftr->frame->pos()).norm(), 2), 1.0, 1.0);
  normal_set_ = true;
}

bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const {
  // TODO: get frame with same point of view AND same pyramid level!
  Vector3d obs_dir(framepos - pos_);
  obs_dir.normalize();
  auto min_it = obs_.begin();
  double min_cos_angle = 0;
  for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it) {
    Vector3d dir((*it)->frame->pos() - pos_);
    dir.normalize();
    double cos_angle = obs_dir.dot(dir);
    if (cos_angle > min_cos_angle) {
      min_cos_angle = cos_angle;
      min_it = it;
    }
  }
  ftr = *min_it;
  if (min_cos_angle <
      0.5)  // assume that observations larger than 60° are useless
    return false;
  return true;
}

void Point::optimize(const size_t n_iter) {
  Vector3d old_point = pos_;
  double chi2 = 0.0;
  Matrix3d A;
  Vector3d b;

  for (size_t i = 0; i < n_iter; i++) {
    A.setZero();
    b.setZero();
    double new_chi2 = 0.0;

    // compute residuals
    for (auto it = obs_.begin(); it != obs_.end(); ++it) {
      Matrix23d J;
      const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J);
      const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
      new_chi2 += e.squaredNorm();
      A.noalias() += J.transpose() * J;
      b.noalias() -= J.transpose() * e;
    }

    // solve linear system
    const Vector3d dp(A.ldlt().solve(b));

    // check if error increased
    if ((i > 0 && new_chi2 > chi2) || (bool)std::isnan((double)dp[0])) {
#ifdef POINT_OPTIMIZER_DEBUG
      cout << "it " << i << "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
#endif
      pos_ = old_point;  // roll-back
      break;
    }

    // update the model
    Vector3d new_point = pos_ + dp;
    old_point = pos_;
    pos_ = new_point;
    chi2 = new_chi2;
#ifdef POINT_OPTIMIZER_DEBUG
    cout << "it " << i << "\t Success \t new_chi2 = " << new_chi2
         << "\t norm(b) = " << vk::norm_max(b) << endl;
#endif

    // stop when converged
    if (vk::norm_max(dp) <= EPS)
      break;
  }
#ifdef POINT_OPTIMIZER_DEBUG
  cout << endl;
#endif
}

}  // namespace svo
